//
// Created by cch on 2022/5/1.
//

#ifndef IMU_GPS_EKF_UTILS_H
#define IMU_GPS_EKF_UTILS_H

#include <eigen3/Eigen/Dense>
#include "../third_party/GeographicLib/include/LocalCartesian.hpp"

namespace IMUGPS {
constexpr double kDegreeToRadian = M_PI/180;
constexpr double kRadianToDegree = 180. / M_PI;
inline void ConvertLLAtoENU(const Eigen::Vector3d &init_lla,
                            const Eigen::Vector3d &point_lla,
                            Eigen::Vector3d &point_enu)
{
    static GeographicLib::LocalCartesian localCartesian;
    localCartesian.Reset(init_lla[0],init_lla[1],init_lla[2]);
    localCartesian.Forward(point_lla(0),point_lla(1),point_lla(2),
                           point_enu.data()[0],point_enu.data()[1],point_enu.data()[2]);

}
inline Eigen::Matrix3d GetSkewMatrix(const Eigen::Vector3d &v) {
  Eigen::Matrix3d w;
  w << 0., -v(2), v(1), v(2), 0., -v(0), -v(1), v(0), 0.;
  return w;
}
} // namespace IMUGPS

#endif // IMU_GPS_EKF_UTILS_H
